/********************************************************
* this file is defined the dynamics identify method
* author : liwang
* date : 20230228
*********************************************************/
#include "identify_dynamics.h"
#include "log/zuclog.h"
#include "nml_intf/zucpos2.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define INFO(fmt, ...) zuclog_info("DYN", fmt, ##__VA_ARGS__)
#define ERR(fmt, ...) zuclog_error("DYN", fmt, ##__VA_ARGS__)
#define DEBUG(fmt, ...) zuclog_debug("DYN", fmt, ##__VA_ARGS__)

#define REPEAT_CYCLE 1

// 动力学参数
static double W1_ = 2.0 * M_PI / 20;
static double W2_ = 8.0 * M_PI / 20;
static double W3_ = 16.0 * M_PI / 20;
static double TSAMPLE = 0.008;
static int CYCLE_NUM = (int)(20 / 0.008);

static FILE* dynamics_identify_file_ = NULL;
static int time_count_ = 0;
static int traj_id_ = 0;
static int repeat_count_ = 0;

// 摩擦力参数
#define VEL_NUM 12
static const double FRICTION_VEL[VEL_NUM] = {1, 5, 10, 20, 40, 60, 80, 100, 120, 140, 160, 180};
static int vel_count_ = 0;
static FILE* friction_identify_file_ = NULL;

void setIdentifyCycleTime(int time)
{
    W1_ = 2.0 * M_PI / time;
    W2_ = 8.0 * M_PI / time;
    W3_ = 16.0 * M_PI / time;
    CYCLE_NUM = (int)(time / TSAMPLE);
}

void resetIdentifyTrajPos()
{
    traj_id_ = 0;
    time_count_ = 0;
    repeat_count_ = 0;
}

int checkIdentifyTimeCount()
{
    if (time_count_ / CYCLE_NUM == traj_id_ + 1 && time_count_ % CYCLE_NUM == 0)
    {  // switch to next traj
        traj_id_++;
    }
    time_count_++;
    if (traj_id_ == IDENTIFY_TRAJ_NUM)
    {  // all traj finished
        traj_id_ = 0;
        time_count_ = 0;
        repeat_count_++;
        if (repeat_count_ == REPEAT_CYCLE)
        {
            repeat_count_ = 0;
            return 1;
        }
    }
    return 0;
}

void GetDynamicsIdenitfyStartPos(const double* a, double* startPos)
{
    // if (zucmotConfig->numJoints > 7)
    // {
    //     return;
    // }
    // double a1[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // double a2[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // double a3[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // double b1[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // double b2[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // double b3[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // double c[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {};
    // for (int j = 0; j < IDENTIFY_TRAJ_NUM; j++)
    // {
    //     for (int k = 0; k < zucmotConfig->numJoints; k++)
    //     {
    //         a1[k][j] = a[k * 7 + j * 7 * zucmotConfig->numJoints];
    //         a2[k][j] = a[k * 7 + 1 + j * 7 * zucmotConfig->numJoints];
    //         a3[k][j] = a[k * 7 + 2 + j * 7 * zucmotConfig->numJoints];
    //         b1[k][j] = a[k * 7 + 3 + j * 7 * zucmotConfig->numJoints];
    //         b2[k][j] = a[k * 7 + 4 + j * 7 * zucmotConfig->numJoints];
    //         b3[k][j] = a[k * 7 + 5 + j * 7 * zucmotConfig->numJoints];
    //         c[k][j] = a[k * 7 + 6 + j * 7 * zucmotConfig->numJoints];
    //     }
    // }

    // double w1 = 2.0 * M_PI * 0.05;
    // double w2 = 8.0 * M_PI * 0.05;
    // double w3 = 16.0 * M_PI * 0.05;
    // for (int i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     // printf("a %d : %lf, %lf, %lf, %lf\n", i, a1[i][0], a2[i][0], a3[i][0], c[i][0]);
    //     startPos[i] = a1[i][0] + a2[i][0] + a3[i][0] + c[i][0];
    //     startPos[i] *= 180.0 / M_PI;
    //     // printf("SP %lf\n", startPos[i]);
    // }
}

void getIdentifyTrajPos(const double* para, double* pos, double t)
{
    // double a1[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};
    // double a2[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};
    // double a3[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};
    // double b1[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};
    // double b2[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};
    // double b3[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};
    // double c[MAX_AXIS][IDENTIFY_TRAJ_NUM] = {{0.0}};

    // for (int j = 0; j < IDENTIFY_TRAJ_NUM; j++)
    // {
    //     for (int k = 0; k < zucmotConfig->numJoints; k++)
    //     {
    //         a1[k][j] = para[k * 7 + j * 7 * zucmotConfig->numJoints];
    //         a2[k][j] = para[k * 7 + 1 + j * 7 * zucmotConfig->numJoints];
    //         a3[k][j] = para[k * 7 + 2 + j * 7 * zucmotConfig->numJoints];
    //         b1[k][j] = para[k * 7 + 3 + j * 7 * zucmotConfig->numJoints];
    //         b2[k][j] = para[k * 7 + 4 + j * 7 * zucmotConfig->numJoints];
    //         b3[k][j] = para[k * 7 + 5 + j * 7 * zucmotConfig->numJoints];
    //         c[k][j] = para[k * 7 + 6 + j * 7 * zucmotConfig->numJoints];
    //     }
    // }

    // for (int i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     pos[i] = a1[i][traj_id_] * cos(W1_ * time_count_ * TSAMPLE) + a2[i][traj_id_] * cos(W2_ * time_count_ * TSAMPLE) +
    //              a3[i][traj_id_] * cos(W3_ * time_count_ * TSAMPLE) + b1[i][traj_id_] * sin(W1_ * time_count_ * TSAMPLE) +
    //              b2[i][traj_id_] * sin(W2_ * time_count_ * TSAMPLE) + b3[i][traj_id_] * sin(W3_ * time_count_ * TSAMPLE) + c[i][traj_id_];
    //     pos[i] *= 180.0 / M_PI;
    // }
}

int recordIdentifyData(int dynamics_identifying_status, int motion_state, const double* q_fb, const double* tor_fb, const double* tor_sensor)
{
    // static int last_dynamics_identifying_status_ = 0;
    // if (dynamics_identifying_status == 1 && last_dynamics_identifying_status_ != 1)
    // {
    //     INFO("Ready open dynamics identification file!\n", motion_state);
    //     dynamics_identify_file_ = fopen("/tmp/dynamics_identify_file.txt", "w+");
    //     if (dynamics_identify_file_ == NULL)
    //     {
    //         INFO("Open Error!\n");
    //     }
    // }
    // else if (dynamics_identifying_status == 1 && last_dynamics_identifying_status_ == 1)
    // {
    //     static int timeout = 0;
    //     if (!GET_MOTION_SERVOMOVEOP_FLAG())
    //     {
    //         timeout++;
    //         if (timeout > 20)
    //         {
    //             INFO("Not int servo mode, mode = %d!\n", motion_state);
    //             return -1;
    //         }
    //     }
    //     else if (dynamics_identify_file_)
    //     {
    //         for (int i = 0; i < zucmotConfig->numJoints; i++) { fprintf(dynamics_identify_file_, "%f\t", q_fb[i]); }
    //         for (int i = 0; i < zucmotConfig->numJoints; i++) { fprintf(dynamics_identify_file_, "%f\t", tor_fb[i]); }
    //         for (int i = 0; i < zucmotConfig->numJoints; i++) { fprintf(dynamics_identify_file_, "%f\t", tor_sensor[i]); }
    //         fprintf(dynamics_identify_file_, "\n");
    //     }
    // }
    // if (dynamics_identifying_status != 1 && dynamics_identify_file_ != NULL)
    // {
    //     fclose(dynamics_identify_file_);
    //     dynamics_identify_file_ = NULL;
    // }
    // last_dynamics_identifying_status_ = dynamics_identifying_status;
    return 0;
}

int recordFriciotnIdentifyData(int friction_identifying_status, int motion_state, const double* q_fb, const double* tor_fb)
{
    // // 又是一次重复计算
    // static double lastjntPosCmd[ZUCMOT_MAX_JOINTS] = {0};
    // static double lastjntVelCmd[ZUCMOT_MAX_JOINTS] = {0};
    // double jntPosCmd[ZUCMOT_MAX_JOINTS] = {0.0};
    // double jntVelCmd[ZUCMOT_MAX_JOINTS] = {0.0};
    // double jntAccCmd[ZUCMOT_MAX_JOINTS] = {0.0};
    // for (int joint_num = 0; joint_num < zucmotConfig->numJoints; ++joint_num)
    // {
    //     jntPosCmd[joint_num] = joints[joint_num].pos_cmd * M_PI / 180.0;  // angle
    //     jntVelCmd[joint_num] = (jntPosCmd[joint_num] - lastjntPosCmd[joint_num]) / 0.008;
    //     jntAccCmd[joint_num] = (jntVelCmd[joint_num] - lastjntVelCmd[joint_num]) / 0.008;
    //     lastjntPosCmd[joint_num] = jntPosCmd[joint_num];
    //     lastjntVelCmd[joint_num] = jntVelCmd[joint_num];
    // }

    // static int last_friction_identifying_status = 0;
    // if (friction_identifying_status == 1 && last_friction_identifying_status != 1)
    // {
    //     INFO("Ready open friction identification file!\n");
    //     char file_name[128];
    //     sprintf(file_name, "%s_%d.txt", "/tmp/friction_identify_file", zucmotStatus->friction_identify_joint_index);
    //     friction_identify_file_ = fopen(file_name, "w+");
    //     if (friction_identify_file_ == NULL)
    //     {
    //         INFO("Open Error!\n");
    //     }
    // }
    // else if (friction_identifying_status == 1 && last_friction_identifying_status == 1)
    // {
    //     if (friction_identify_file_ && fabs(jntAccCmd[zucmotStatus->friction_identify_joint_index] < 0.001))  // 取匀速段数据
    //     {
    //         for (int i = 0; i < zucmotConfig->numJoints; i++) { fprintf(friction_identify_file_, "%f\t", q_fb[i]); }
    //         for (int i = 0; i < zucmotConfig->numJoints; i++) { fprintf(friction_identify_file_, "%f\t", tor_fb[i]); }
    //         fprintf(friction_identify_file_, "\n");
    //     }
    // }
    // if (friction_identifying_status != 1 && friction_identify_file_ != NULL)
    // {
    //     fclose(friction_identify_file_);
    //     friction_identify_file_ = NULL;
    // }
    // last_friction_identifying_status = friction_identifying_status;
    return 0;
}

void resetFrictionTraj() { vel_count_ = 0; }

static double GetMaxVel(int jnt_id, const double* start_pos, const double* end_pos)
{
    // double delta_pos = 0;
    // delta_pos = fabs(end_pos[jnt_id] - start_pos[jnt_id]);
    // double max_acc = joints[jnt_id].acc_limit;
    // double max_vel = sqrt(2.0 * max_acc * delta_pos / 2.0);
    // max_vel = max_vel > joints[jnt_id].vel_limit ? max_vel : joints[jnt_id].vel_limit;
    // return max_vel;
    return 0;
}

static int sign(double a)
{
    if (a > 0)
        return 1;
    else if (a < 0)
        return -1;
    else
        return 0;
}

int friction_identify_moving()
{
    // static double max_vel = 0;
    // static short last_friction_identifying_status = 0;
    // int jnt_id = zucmotStatus->friction_identify_joint_index;
    // double start_pos[ZUCMOT_MAX_JOINTS] = {0};
    // double end_pos[ZUCMOT_MAX_JOINTS] = {0};
    // double ds[ZUCMOT_MAX_JOINTS] = {0};
    // for (int i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     start_pos[i] = zucmotStatus->identify_friction_start_pos[i + jnt_id * zucmotConfig->numJoints];
    //     end_pos[i] = zucmotStatus->identify_friction_end_pos[i + jnt_id * zucmotConfig->numJoints];
    //     ds[i] = end_pos[i] - start_pos[i];
    // }
    // if (zucmotStatus->friction_identifying_status == 1 && last_friction_identifying_status == 0)
    // {
    //     max_vel = GetMaxVel(jnt_id, start_pos, end_pos);  // 计算允许最大加速度，以及用户规定行程下的最大可达速度
    //     INFO("Joint %d Max_ACC %lf MAX_VEL %lf\n", jnt_id, joints[jnt_id].acc_limit, max_vel);
    // }
    // last_friction_identifying_status = zucmotStatus->friction_identifying_status;
    // if (zucmotStatus->friction_identifying_status != 1)
    // {
    //     return 0;
    // }
    // if (GET_MOTION_INPOS_FLAG())
    // {
    //     for (int jntId = 0; jntId < zucmotConfig->numJoints; ++jntId)
    //     {
    //         if (vel_count_ & 1)
    //         {
    //             double dds =
    //                 (FRICTION_VEL[vel_count_ / 2]) * (FRICTION_VEL[vel_count_ / 2]) / 2.0 / joints[jntId].acc_limit + fabs(FRICTION_VEL[vel_count_ / 2]) * 4;
    //             dds = dds < fabs(ds[jntId]) ? dds : fabs(ds[jntId]);
    //             joints[jntId].free_tp.pos_cmd = start_pos[jntId] + sign(ds[jntId]) * dds;
    //         }
    //         else
    //         {
    //             joints[jntId].free_tp.pos_cmd = start_pos[jntId];
    //         }
    //         joints[jntId].free_tp.max_vel = FRICTION_VEL[vel_count_ / 2];
    //         joints[jntId].free_tp.max_acc = joints[jntId].acc_limit;
    //         joints[jntId].kb_jjog_active = 1;
    //         joints[jntId].free_tp.enable = 1;
    //         SET_JOINT_ERROR_FLAG(&joints[jntId], 0);
    //         clearHomes(jntId);
    //     }
    //     INFO("Current pos : %lf %lf %lf %lf %lf %lf\n",
    //          joints[0].pos_fb,
    //          joints[1].pos_fb,
    //          joints[2].pos_fb,
    //          joints[3].pos_fb,
    //          joints[4].pos_fb,
    //          joints[5].pos_fb);
    //     INFO("target pos : %lf %lf %lf %lf %lf %lf\n",
    //          joints[0].free_tp.pos_cmd,
    //          joints[1].free_tp.pos_cmd,
    //          joints[2].free_tp.pos_cmd,
    //          joints[3].free_tp.pos_cmd,
    //          joints[4].free_tp.pos_cmd,
    //          joints[5].free_tp.pos_cmd);
    //     SET_MOTION_INPOS_FLAG(0);
    //     INFO("current idenitfy velocity is : %lf, vel_count_ = %d\n", FRICTION_VEL[vel_count_ / 2], vel_count_);
    //     vel_count_++;
    // }
    // if (FRICTION_VEL[vel_count_ / 2] > max_vel || vel_count_ > VEL_NUM * 2 - 2)
    // {
    //     vel_count_ = 0;
    //     zucmotStatus->friction_identifying_status = 2;
    //     return 2;
    // }
    return 0;
}